package remoteControlTest;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.Motor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;

public class RemoteControl {

	public static boolean run = true;
	/**
	 * @param args
	 * @throws IOException 
	 */
	
	public static BTConnection c;
	public static DataInputStream in;
	public static DataOutputStream out;
	
	public static void main(String[] args) throws IOException {
		setUpConnection();	
		remoteControl();
		closeConnection();
	}
	
	public static void setUpConnection() {
		System.out.println("Waiting for incomming connection...");
		c = Bluetooth.waitForConnection();
		in = c.openDataInputStream();
		out = c.openDataOutputStream();
		
	
		System.out.println("Connection established!");
		System.out.println("waiting for commands");
		
		
		
		Button.ESCAPE.addButtonListener(new ButtonListener(){

			public void buttonPressed(Button arg0) {
				stop();
			}
			public void buttonReleased(Button arg0) {	
			}
			
		});
	}
	
	public static void remoteControl() throws IOException {
		
		byte[] b = new byte[12];
		
		while(run){
			in.read(b);	
			
			int portA = byteArrayToInt(b, 0);
			int portB = byteArrayToInt(b, 4);
			int portC = byteArrayToInt(b, 8);
			
			if(portA == 666 && portB == 666 && portC == 666)
				closeConnection();
			
			setMotor(Motor.A, portA);
			setMotor(Motor.B, portB);
			setMotor(Motor.C, portC);
			
		}
	}
	
	
	public static void stop(){
		run = false;
	}
	
	public static void setMotor(Motor motor, int speed){
		if(speed > 0){
			motor.setSpeed(speed);
			motor.forward();
		}
		else if(speed < 0){
			motor.setSpeed(speed);
			motor.backward();
		}else{
			motor.stop();
		}
	}
	
	public static void closeConnection(){
		try {
			in.close();
			out.close();
		} catch (IOException e) {
		}

		c.close();
		System.exit(0);
	}
	
	public static int byteArrayToInt(byte [] b, int offset) {
        return (b[offset] << 24)
                + ((b[offset+1] & 0xFF) << 16)
                + ((b[offset+2] & 0xFF) << 8)
                + (b[offset+3] & 0xFF);
	}


}